#ifndef SD_MINBALL_HPP
#define SD_MINBALL_HPP

#include "sd/ball.hpp"
#include "sd/linalg.hpp"
#include "real.hpp"
#include <vector>
#include <list>
#include <iostream>

namespace MT {

    // Class Definitions
    // =================

    // minball_core
    // ----------
    // smallest ball with a set of n <= d+1 points *on the boundary*
	template<typename _Scalar, int _Rows>
    class minball_core {
    private:
		static const int _Rows1 = _Rows==Eigen::Dynamic? Eigen::Dynamic: (_Rows+1);

        // data members
        int d;      // dimension
        int m, s;   // size and number of support points

        Eigen::Matrix<_Scalar, _Rows, 1> q0;
		Eigen::Matrix<_Scalar, _Rows1, 1> z, f;
        Eigen::Matrix<_Scalar, _Rows, _Rows1> v, c;

        Eigen::Matrix<_Scalar, _Rows1, 1> sqr_r;

        int current_c_index;      // index to refer to some c.col(j)
        double current_sqr_r;

    public:
        minball_core(int dim)
			: d(dim), q0(dim, 1), z(dim+1, 1), f(dim+1, 1), v(dim, dim+1), c(dim, dim+1), sqr_r(dim+1, 1)
		{
			current_c_index = 0;
			reset();
		}


        // access
        inline const Eigen::Matrix<_Scalar, _Rows, 1> center() const { return c.col(current_c_index); }
        inline double squared_radius() const { return current_sqr_r; }
        inline int size() const { return m; }
        inline int support_size() const { return s; }
        inline double excess(const Eigen::Matrix<_Scalar, _Rows, 1>& p) const
			{ return (p-c.col(current_c_index)).squaredNorm()-current_sqr_r; }

        // modification
        inline void reset() // generates empty sphere with m=s=0
		{
			m = s = 0;
			// we misuse c.col(0) for the center of the empty sphere
			c.col(0).fill(0);
			current_c_index = 0;
			current_sqr_r = -1;
		}

        bool push(const Eigen::Matrix<_Scalar, _Rows, 1>& p)
		{
			double eps = 1e-32;
			if (!m) {
				q0 = p;
				c.col(0) = q0;
				sqr_r(0) = 0;
			} else {
				// set v_m to Q_m
				Eigen::Matrix<_Scalar, _Rows, 1> v_m(p-q0);

                if(m > 1)
                {
                    Eigen::Block<Eigen::Matrix<_Scalar, _Rows, _Rows1> > v_b(v.block(0,1,d,m-1));

                    // compute the a_{m,i}, i< m
                    Eigen::Matrix<_Scalar, Eigen::Dynamic, 1> a_m(v_b.transpose()*v_m);
                    a_m = a_m.array() * 2 / z.segment(1,m-1);

                    // update v_m to Q_m-\bar{Q}_m
                    v_m.noalias() -= v_b*a_m;
                }

				// compute z_m
				z(m) = v_m.squaredNorm()*2;

				// reject push if z_m too small
				if (z(m)<eps*current_sqr_r) return false;

				// update c, sqr_r
				double e = (p-c.col(m-1)).squaredNorm()-sqr_r(m-1);
				f(m)=e/z(m);

				c.col(m) = c.col(m-1) + f(m)*v_m;
				sqr_r(m) = sqr_r(m-1) + e*f(m)/2;

				// set v_m to real v_m
				v.col(m) = v_m;
			}

			current_c_index = m;
			current_sqr_r = sqr_r(m);
			s = ++m;
			return true;
		}

        inline void pop () { --m; }
    };




    // minball
    // ---------
    // smallest enclosing ball of a set of n points in dimension d
	template<typename _Scalar, int _Rows>
    class minball {
    public:
        // types
        typedef typename std::list<Eigen::Matrix<_Scalar, _Rows, 1> >::iterator         It;
        typedef typename std::list<Eigen::Matrix<_Scalar, _Rows, 1> >::const_iterator   Cit;

    private:
        // data members
        int  d;                 // dimension
        std::list<Eigen::Matrix<_Scalar, _Rows, 1> > L;  // internal point set
        minball_core<_Scalar, _Rows> B;            // the current ball
        It support_end;         // past-the-end iterator of support set

        // private methods
        void mtf_mb(It i)
		{
			support_end = L.begin();
			if ((B.size())==d+1) return;
			for (It k=L.begin(); k!=i;) {
				It j=k++;
				if (B.excess(*j) > 0) {
					if (B.push(*j)) {
						mtf_mb (j);
						B.pop();
						move_to_front(j);
					}
				}
			}
		}

		void pivot_mb(It i)
		{
			It t = ++L.begin();
			mtf_mb (t);
			double max_e, old_sqr_r = -1;
			do {
				It pivot;
				max_e = max_excess (t, i, pivot);
				if (max_e > 0) {
					t = support_end;
					if (t==pivot) ++t;
					old_sqr_r = B.squared_radius();
					B.push (*pivot);
					mtf_mb (support_end);
					B.pop();
					move_to_front (pivot);
				}
			} while ((max_e > 0) && (B.squared_radius() > old_sqr_r));
		}

        inline void move_to_front(It j)
		{
			if (support_end == j) support_end++;
			L.splice (L.begin(), L, j);
		}

        double max_excess(It t, It i, It& pivot) const
		{
			const double sqr_r = B.squared_radius();
			double e, max_e = 0;
			for (It k=t; k!=i; ++k) {
				e = (*k-B.center()).squaredNorm()-sqr_r;
				if (e > max_e) {
					max_e = e;
					pivot = k;
				}
			}
			return max_e;
		}

    public:
        // creates an empty ball
        minball(int dim) : d(dim), B(dim) {}

        // copies p to the internal point set
        inline void check_in(const Eigen::Matrix<_Scalar, _Rows, 1>& p)
        {
            ei_assert (d == (int) p.size() && "Wrong input dimensionality.");
            L.push_back(p);
        }

        // builds the smallest enclosing ball of the internal point set
        inline void build()
		{
			B.reset();
			support_end = L.begin();
			pivot_mb (L.end());
		}

        // returns center of the ball (undefined if ball is empty)
        inline const Eigen::Matrix<_Scalar, _Rows, 1> center() const { return B.center(); }

        // returns squared_radius of the ball (-1 if ball is empty)
        inline double squared_radius () const { return B.squared_radius(); }

        // returns size of internal point set
        inline int nr_points () const { return (int)L.size(); }

        // returns begin- and past-the-end iterators for internal point set
        inline Cit points_begin () const { return L.begin(); }
        inline Cit points_end () const { return L.end(); }

        // returns size of support point set; this set has the following properties:
        // - there are at most d+1 support points,
        // - all support points are on the boundary of the computed ball, and
        // - the smallest enclosing ball of the support point set equals the
        //   smallest enclosing ball of the internal point set
        inline int nr_support_points () const { return B.support_size(); }

        // returns begin- and past-the-end iterators for internal point set
        inline Cit support_points_begin () const { return L.begin(); }
        inline Cit support_points_end () const { return support_end; }
    };

// find an approximate minimum bounding ball -- cheap solution
template<typename _Scalar, int _Rows>
ball<_Scalar, _Rows> approx_minball(Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic> const &points)
{
	int N = points.cols();
	Eigen::Matrix<_Scalar, _Rows, 1> mean(points.rowwise().sum()/N);
	_Scalar r = (points.colwise()-mean).colwise().norm().maxCoeff();
	return ball<_Scalar, _Rows>(mean, r);
}

// find an exact minimum bounding ball -- cheap solution
template<typename _Scalar, int _Rows>
ball<_Scalar, _Rows> exact_minball(Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic> const &points)
{
	int d = points.rows();
	int N = points.cols();
	minball<_Scalar, _Rows> mb(d);
	for(int i = 0; i < N; ++i)
		mb.check_in(points.col(i));
	mb.build();
	return ball<_Scalar, _Rows>(mb.center(), sqrt(mb.squared_radius()));
}


} // namespace MT

#endif
